﻿/* Copyright 2007-2008 dnAnalytics Project.
 *
 * Contributors to this file:
 * Patrick van der Velde
 * Marcus Cuda - http://marcus.cuda.net
 *
 * This file is part of dnAnalytics.  dnAnalytics is licensed under the 
 * Microsoft Public License. See License.txt for a complete copy of the
 * license.
 */

using System;

namespace dnAnalytics.LinearAlgebra.Decomposition
{
    internal class DenseCholesky : AbstractCholesky
    {
        public DenseCholesky(Matrix matrix) : base(matrix) {}

        protected override bool DoCompute(Matrix matrix, int order)
        {
            double[] data = ((DenseMatrix) matrix).Data;
            double[] factor = new double[data.Length];
            for (int j = 0; j < order; j++)
            {
                double d = 0.0;
                int index;
                for (int k = 0; k < j; k++)
                {
                    double s = 0.0;
                    int i;
                    for (i = 0; i < k; i++)
                    {
                        s += factor[i*order + k]*factor[i*order + j];
                    }
                    int tmp = k*order;
                    index = tmp + j;
                    factor[index] = s = (data[index] - s)/factor[tmp + k];
                    d += s*s;
                }
                index = j*order + j;
                d = data[index] - d;
                if (d <= 0.0)
                {
                    return false;
                }
                factor[index] = System.Math.Sqrt(d);
                for (int k = j + 1; k < order; k++)
                {
                    factor[k*order + j] = 0.0;
                }
            }
            Buffer.BlockCopy(factor, 0, data, 0, factor.Length*Constants.SizeOfDouble);
            return true;
        }

        protected override void DoSolve(Matrix factor, Matrix result)
        {
            Solve(((DenseMatrix)factor).Data, ((DenseMatrix)result).Data, factor.Rows, result.Columns);
        }

        protected override void DoSolve(Matrix factor, Vector result)
        {
            Solve(((DenseMatrix) factor).Data, ((DenseVector)result).Data, factor.Rows, 1);
        }

        private static void Solve(double[] data, double[] rhs, int order, int columns)
        {
            for (int c = 0; c < columns; c++)
            {
                int cindex = c*order;
                // Solve L*Y = B;
                double sum;
                for (int i = 0; i < order; i++)
                {
                    sum = rhs[c*order + i];
                    for (int k = i - 1; k >= 0; k--)
                    {
                        sum -= data[k*order + i]*rhs[cindex + k];
                    }
                    rhs[cindex + i] = sum/data[i*order + i];
                }

                // Solve L'*X = Y;
                for (int i = order - 1; i >= 0; i--)
                {
                    sum = rhs[cindex + i];
                    int iindex = i*order;
                    for (int k = i + 1; k < order; k++)
                    {
                        sum -= data[iindex + k]*rhs[cindex + k];
                    }
                    rhs[cindex + i] = sum/data[iindex + i];
                }
            }
        }
    }
}
